/* 
 * File:   qmath.h
 * Author: bram
 *
 * Created on December 8, 2013, 4:48 AM
 */

#ifndef QMATH_H
#define	QMATH_H

#ifdef	__cplusplus
extern "C" {
#endif


#include "types.h"


#define Q_DEG_TO_RAD		1144
#define Q_RAD_TO_DEG		3754936

#define Q_SIN_LUT_COUNT 4341

#define Q_SIN_OVERFLOW 10

#define Q_SIN_LUT_MULTIPLIER 7
#define Q_SIN_LUT_DIV 166
#define Q_PI 205887
#define Q_PI_DIV2 102944
#define Q_ONE 65536
#define Q_HALF (Q_ONE/2)

void vectorScale(q_vector_t * v, q_t s);
void vectorNormalize(q_vector_t * v);
void vectorTranslate(q_vector_t * vector, q_vector_t * translation);
void matrixMultiply(q_matrix_t * a, q_matrix_t * b, q_matrix_t * c);
__inline void matrixMultiplyVector(const q_matrix_t * a, const q_vector_t * b, q_vector_t * c);
void matrixAxis(const  q_vector_t * u, q_matrix_t * m);


#define Q_FRACBITS          15
#define Q_FRACVAL           (2 << Q_FRACBITS)

#define QQ_FRACBITS         23
#define QQ_FRACVAL         (2 << QQ_FRACBITS)

#define FLOAT_TO_Q(f)		(q_t)((f) * Q_FRACVAL)
#define FLOAT_TO_QQ(f)		(qq_t)((f) * QQ_FRACVAL)

#define Q_TO_QQ(q)          (qq_t)(q * (QQ_FRACVAL / Q_FRACVAL))
#define QQ_TO_Q(q)          (q_t)(q / (QQ_FRACVAL / Q_FRACVAL))

#define qAdd(a,b)           (q_t)( (a)+(b) )
#define qMul(a,b)           (q_t)   ( ((((long long)(a)*(b)+Q_FRACVAL/2 ))) >> (Q_FRACBITS +1))
#define qMul3(a,b,c)        (qMul(a,qMul(b,c)))

#define qqAdd(a,b)          qAdd(a,b)
#define qqMul(a,b)          (qq_t)  ( ((((long long)(a)*(b)+QQ_FRACVAL/2))) >> (QQ_FRACBITS+1))
#define qqMul3(a,b,c)       qMul3(a,b,c)

#define qDiv(a,b)           (q_t)(((long long)(a) << (Q_FRACBITS+1)) / (b))
#define qqDiv(a,b)          (qq_t)(((long long)(a) << (QQ_FRACBITS+1)) / (b))

#define qRnd(a)             (q_t)( (((a)+((a) > 0 ? Q_FRACVAL / 2 : Q_FRACVAL / 2 - 1)) & 0xFF000000))
#define qqRnd(a)            (qq_t)( (((a)+((a) > 0 ? QQ_FRACVAL / 2 : QQ_FRACVAL / 2 - 1)) & 0xFF000000))

#define qAbs(a)             (q_t)( (a) > 0 ? (a) : -(a))
#define qqAbs(a)            (qq_t)(qAbs(a))

#define qPow2(a)            (qMul(a,a))
#define qqPow2(a)           (qqMul(a,a))

#define vectorNormalize_inline(v) { q_t norm_length_inline = qDiv(Q_ONE, vectorLength_inline(v)); \
	vectorScale_inline(v, norm_length_inline); \
}

#define vectorScale_inline(v, s) { v.x = qMul(v.x,s); v.y = qMul(v.y,s); v.z = qMul(v.z,s); }

q_t	qSqrt(q_t a);
q_t qAtan2(q_t a, q_t b);
q_t qAsin(q_t a);


#define vectorLength_inline(v) qSqrt(qAdd(qAdd(qMul(v.x,v.x),qMul(v.y,v.y)),qMul(v.z,v.z)))


#ifdef	__cplusplus
}
#endif

#endif	/* QMATH_H */

